package estrategia;

import java.io.IOException;
import javax.vecmath.Point3d;
import comunicacion.Conector;

public class CamposPotencial {
	
	private final int k1 = 50000;
	private final int k2 = 3200;
	private final double TOLERANCIA_PARED = 0.01;
	private final double PARED_ARRIBA = 1.79 - TOLERANCIA_PARED;
	private final double PARED_DER = 2.2 - TOLERANCIA_PARED;
	private final double PARED_IZQ = TOLERANCIA_PARED;
	private final double PARED_ABAJO = TOLERANCIA_PARED;
	private final double SESGO_RADIO = 0.12;
	private final double TOPE_RADIO = 0.6;
	
	
	public CamposPotencial() {
	}

	
	// Devuelve la distancia minima de las paredes para tomar en cuenta en el calculo del radio
	public double calcularMinimaDistanciaParedes(Point3d p) {
		double min1 = Double.MAX_VALUE;
		double min2 = Double.MAX_VALUE;
		if (PARED_ARRIBA - p.y > p.y - PARED_ABAJO)
			min1 = p.y - PARED_ABAJO;
		else min2 = PARED_ARRIBA - p.y;
		
		if (PARED_DER - p.x > p.x - PARED_IZQ)
			min1 = p.x - PARED_IZQ;
		else min2 = PARED_DER - p.x;
		
		return Math.min(min1, min2);
	}
	
	
	// Devuelve la distancia minima de un obstaculo para tomar en cuenta en el radio de
	// la figura que calcula la fuerza y lso campos de potencial 
	public double calcularDistanciaMinimaObstaculos(Point3d miPos, Point3d[] obstaculos) {
		double distMinima = Float.MAX_VALUE;
		double dist;
		for (int i=0; i<obstaculos.length; i++) {
			dist = getDistancia(miPos, obstaculos[i]);
			if (dist < distMinima)
				distMinima = dist;
		}
		
		dist = calcularMinimaDistanciaParedes(miPos); 
		if ( dist < distMinima)
			distMinima = dist;
		
		if (distMinima > TOPE_RADIO)
			distMinima = TOPE_RADIO;
		
		return distMinima - SESGO_RADIO;
	}
	
	
	// Devuelvo la posicion de la celda basado en una figura de rombo
	// El orden es en sentido horario
	// los puntos que se toman del rombo son los extremos y dos puntos al centro de cada
	// arista correspondiente al rombo
	public Point3d getPosicionCelda(int celda, Point3d mipos, double radio) {
		double paso = (radio/2) - 0.08;
		double paso2 = 0.15;
		switch (celda) {
			case 0: return new Point3d(mipos.x, mipos.y+radio, 0);
			case 1: return new Point3d(mipos.x+paso, mipos.y+radio-paso, 0);
			case 2: return new Point3d(mipos.x+paso+paso2, mipos.y+radio-paso-paso2, 0);
			case 3: return new Point3d(mipos.x+radio, mipos.y, 0);
			
			case 4: return new Point3d(mipos.x+radio-paso, mipos.y-paso, 0);
			case 5: return new Point3d(mipos.x+radio-paso-paso2, mipos.y-paso-paso2, 0);
			case 6: return new Point3d(mipos.x, mipos.y-radio, 0);
			
			case 7: return new Point3d(mipos.x-paso, mipos.y-radio+paso, 0);
			case 8: return new Point3d(mipos.x-paso-paso2, mipos.y-radio+paso+paso2, 0);
			case 9: return new Point3d(mipos.x-radio, mipos.y, 0);
			
			case 10: return new Point3d(mipos.x-radio+paso, mipos.y+paso, 0);
			case 11: return new Point3d(mipos.x-radio+paso+paso2, mipos.y+paso+paso2, 0);
			default: return new Point3d(0, 0, 0);
		}
	}
	
	public double getDistancia(Point3d a, Point3d b) {
		double distX = Math.abs(a.x - b.x);
		double distY = Math.abs(a.y - b.y);
		
		return Math.sqrt(Math.pow(distX , 2) + Math.pow(distY, 2));
	}
	
	
	public Point3d hallarProximoDestino(Point3d origen, Point3d objetivo, Point3d[] obstaculos) {
		double Fres = 0;
		double Fcelda;
		Point3d res3D = null;
		Point3d posCelda = null;
		
		// Calculo la distancia minima de todos los obstaculos para hallar el radio
		double radio = calcularDistanciaMinimaObstaculos(origen, obstaculos);
		
		if (getDistancia(origen, objetivo) < radio) {
			res3D = new Point3d(objetivo.x, objetivo.y, 0);
			Fres = -1;
			
		}else {
			//Calculo Fuerza maxima y obtengo punto destino
			for (int celda=0; celda<12; celda++) {
				posCelda = getPosicionCelda(celda, origen, radio);
				double distObjetivo = (getDistancia(posCelda, objetivo));
				Fcelda = k1/ distObjetivo;
				
				for (int idObs=0; idObs<obstaculos.length; idObs++) {
					double Fobstaculo = (k2/getDistancia(posCelda, obstaculos[idObs]));
					Fcelda = Fcelda - Fobstaculo;//sumaVectores(Fcelda, Fobstaculo);
				}
				if ( Fcelda > Fres) {
					Fres = Fcelda;
					res3D = new Point3d(posCelda.x, posCelda.y, 0);
				}
			}
		}
		
		return res3D; 
	}	
		
}
